﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using System.Windows.Media.Imaging;
using System.IO;
using System.Windows.Media;
using System.Windows;

using Biomedica.Graphics;
using System.Windows.Controls;
using NuiDicomPlayer.VirtualMouse;
using NuiDicomPlayer;
using System.Runtime.CompilerServices;
using NuiDicomPlayer.Nui.Gestures;
using NuiDicomPlayer.Nui.NavigationEngine;
using Biomedica.Screen.Mappers;
using NuiDicomPlayer.DSP;
using NuiDicomPlayer.VirtualMouse.Kinect;



namespace KinectLibrary
{
    public delegate void DepthFrameReady(WriteableBitmap bmp);
    /// <summary>
    /// This is the manager that will handle all the kineckt life cycle
    /// </summary>
    public  class KinectGestureManager : IDisposable
    {
        #region SingletonConstructor
        protected static KinectGestureManager kinectGestureManager;
        [MethodImpl(MethodImplOptions.Synchronized)]
        public static KinectGestureManager New()
        {
            if (kinectGestureManager == null) return new KinectGestureManager();
            else return kinectGestureManager;
        }
        #endregion
        #region Proparties
        public Canvas canvas { set; get; }
        public Boolean EmitMultiSkeleton { set; get; }
        public Window MainWindow{set;get;}
        public MouseController _mouseController { set; get; } 
        public WriteableBitmap Bmp { get; set; }
        public DepthFrameReady OnDepthFrameReady { set; get; }
        public bool MotorScanning { get; set; }
        private ScreencordMapper Screen_mapper;
        int framcount;
        Vector2 savedFilterdJointPostion;
        Vector2 savedTrend;
        Vector2 savedBasePostion;

        const int buffersize = 5;

        Filter _filterR = new Filter();
        Filter _filterL = new Filter();

        DSP LH_kNormaliser_X = new DSP(buffersize);
        DSP RH_kNormaliser_X = new DSP(buffersize);
        DSP LH_kNormaliser_Y = new DSP(buffersize);
        DSP RH_kNormaliser_Y = new DSP(buffersize);

        public float TrendSmoothFactoe { set; get; }
        public float JitterRadious { set; get; }
        public float DataSmothingFactor { set; get; }
        public float PredicationFacrtor { set; get; }
        public float GlobalSmooth { set; get; }

        GesturPostureEngine eng = new GesturPostureEngine();
        public List<Skeleton> SkeletonCollection
        {
            get;
            set;
        }
        public bool IsLeftHanded { get; set; }


        /// <summary>
        /// Set the Active Working Sensor
        /// </summary>
        public int ActiveSensor
        {
            set
            {
                if (value > KinectSensor.KinectSensors.Count)
                { 
                    activesensor = -1;
                }
                else activesensor = value;
            }

            get
            {
                return activesensor;
            }
        }
        /// <summary>
        /// Enable The Active Kineckt Sensor
        /// </summary>
        public bool EnableKinect
        {
            set
            {
                if (value)
                {
                   

                    if (ActiveSensor != -1)
                    {
                        var parameters = new TransformSmoothParameters
                        {
                            Correction = 0.9f,
                            Prediction = 0.5f,
                            Smoothing = 0.05f,
                            JitterRadius = 0.8f,
                            MaxDeviationRadius = 0.2f
                        };
                        KinectSensor.KinectSensors[ActiveSensor].SkeletonFrameReady += KinectGestureManager_SkeletonFrameReady;
                        KinectSensor.KinectSensors[ActiveSensor].DepthStream.Enable();
                        KinectSensor.KinectSensors[ActiveSensor].SkeletonStream.Enable(parameters);
                        KinectSensor.KinectSensors[ActiveSensor].Start();

                    }
                    else
                    {
                        MessageBoxResult result = MessageBox.Show("There is no Kinect detected, please be sure that the kinect is connected and rightly configured, the program will shutdown now");
                        Environment.Exit(0);
                    }
                }
                else
                {
                   // KinectSensor.KinectSensors[activesensor].DepthStream.Disable();
                    //KinectSensor.KinectSensors[activesensor].SkeletonStream.Disable();
                    KinectSensor.KinectSensors[activesensor].Stop();
                }
            }
        }

        #endregion
        #region Feilds
       Int32   activesensor;
        byte[] depthFrame32;
        #endregion
      

        /// <summary>
        /// Construct a new Kineckt Sensor Manager
        /// </summary>
        public KinectGestureManager()
        {
           
            eng.Limit = 60;
            TrendSmoothFactoe = 0.25f;
            JitterRadious = 0.05f;            
            DataSmothingFactor = 0.5f;
            PredicationFacrtor = 0.5f;
            savedFilterdJointPostion = Vector2.Zero;
            savedTrend = Vector2.Zero;
            savedBasePostion = Vector2.Zero;
            framcount = 0;
           
        }
        /// <summary>
        /// Triggerd when Skeleton Fram is ready
        /// </summary>
        /// <param name="sender">The sender Object</param>
        /// <param name="e">Events Parameters</param>
        void KinectGestureManager_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            using(SkeletonFrame frame = e.OpenSkeletonFrame())
            {
            if (frame != null)
            {
                Skeleton[] skeletons = new Skeleton[frame.SkeletonArrayLength];
                frame.CopySkeletonDataTo(skeletons);
                if (skeletons.Length > 0) AnalyseSkeleton(skeletons); 
                frame.Dispose();
            }   
            }
        }

     
           /// <summary>
        /// Set the Sensot angle in any value between -30 and 30
        /// </summary>
        /// <param name="angle">The angle to til</param>
        public void SetSensorTilt(double angle)
        {
            if (angle > -30 && angle < 30) 
                KinectSensor.KinectSensors[activesensor].ElevationAngle = (int)angle;
        }
        /// <summary>
        /// Perform Skeleton Analaysis
        /// </summary>
        /// <param name="skeletons">Collection of all the skeletons in the stage</param>
        private void AnalyseSkeleton(Skeleton[] skeletons)
        {

            for (int i = 0; i < skeletons.Length; i++)
            {
                if (skeletons[i].TrackingState != SkeletonTrackingState.Tracked)
                {
                    continue;
                }

                Screen_mapper = new ScreencordMapper(canvas);
                
                Vector2 cog_postion = (new Vector2(skeletons[i].Joints[JointType.ShoulderLeft].Position.X,
                                            skeletons[i].Joints[JointType.ShoulderLeft].Position.Y));

                //perform Mapping
               // Vector2 cog_postion = new Vector2(skeletons[i].Position.X, skeletons[i].Position.Y);

                

                Vector2 left_hand =  (new Vector2(skeletons[i].Joints[JointType.HandLeft].Position.X,
                                              skeletons[i].Joints[JointType.HandLeft].Position.Y) );

                Vector2 right_hand = (new Vector2(skeletons[i].Joints[JointType.HandRight].Position.X,
                                             skeletons[i].Joints[JointType.HandRight].Position.Y) ); 

                Vector2 head = ( new Vector2(skeletons[i].Joints[JointType.Head].Position.X,
                                             skeletons[i].Joints[JointType.Head].Position.Y)); 

                Vector2 spine = ( new Vector2(skeletons[i].Joints[JointType.Spine].Position.X,
                                             skeletons[i].Joints[JointType.Spine].Position.Y));
                Screen_mapper.SmothingFactor = 1.65;
                Screen_mapper.XBound = App.ActiveMouse.ActualWidth+10;
                Screen_mapper.YBound = App.ActiveMouse.ActualHeight+10;

                Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, left_hand.X, left_hand.Y);
                left_hand.X = (float)Screen_mapper.X;
                left_hand.Y = (float)Screen_mapper.Y;



                
               
                Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, right_hand.X, right_hand.Y);
                right_hand.X = (float)Screen_mapper.X;
                right_hand.Y = (float)Screen_mapper.Y;


                

                Vector2 v1 =Vector2.Zero;
                Vector2 v2 = Vector2.Zero;



                v2 = _filterL.FilterJointPostion(Microsoft.Kinect.KinectSensor.KinectSensors[activesensor],  left_hand);
                v1 = _filterR.FilterJointPostion(Microsoft.Kinect.KinectSensor.KinectSensors[activesensor], right_hand);
                
                v2.X = (float)LH_kNormaliser_X.ExponentialWeightedAvg(v2.X);
                v2.Y = (float)LH_kNormaliser_Y.ExponentialWeightedAvg(v2.Y);
                v1.X = (float)RH_kNormaliser_X.ExponentialWeightedAvg(v1.X);
                v1.Y = (float)RH_kNormaliser_Y.ExponentialWeightedAvg(v1.Y);
                App.ActiveMouse.Enabled = true;
                App.ActiveMouse.Location = new Point(v1.X, v1.Y);
                App.SubActiveMouseVector = v2;
                App.ActiveMouse.UpdateHand(v1, canvas,App.ActiveWindow);
               

                eng.Push(skeletons[i], TimeSpan.FromTicks(DateTime.Now.Ticks).Ticks);
                App.Nav.StartPostureDetectionCycle(eng);
            }
          }

        private void SetHandPostion2(KinectSensor kinectSensor, Vector2 hand, out Vector2 v)
        {

            double dx = canvas.ActualWidth / 2+(hand.X * (canvas.ActualWidth / 2)) ;
            double dy = canvas.ActualWidth / 2-(hand.Y * (canvas.ActualHeight / 2));
           
           v= new Vector2((float)dx, (float)dy);
        }
       





        public Vector2 FilterJointPostion(Vector2 Joint)
        {
            var sensor = Microsoft.Kinect.KinectSensor.KinectSensors[activesensor];
            Vector2 filteredJointPostion = Vector2.Zero;
            Vector2 differanceVector = Vector2.Zero;
            Vector2 currentTrend = Vector2.Zero;
            Vector2 baseJointPostion = Joint;
            Vector2 previousFilterdJointPostion = savedFilterdJointPostion;
            Vector2 previousTrens = savedTrend;

            Vector2 previousBaseJointPostion = savedBasePostion;

            switch (framcount)
            {
                case (0):
                    filteredJointPostion = baseJointPostion;
                    currentTrend = Vector2.Zero;
                    break;
                case (1):
                    filteredJointPostion = (baseJointPostion + previousBaseJointPostion) * 0.5;
                    differanceVector = filteredJointPostion - previousBaseJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

                default:

                    differanceVector = baseJointPostion - previousFilterdJointPostion;


                    float distance = differanceVector.Distance();
                    if (distance <= JitterRadious)
                        filteredJointPostion = baseJointPostion * (distance / JitterRadious) + previousFilterdJointPostion * (1.0f) - (distance / JitterRadious);
                    else
                        filteredJointPostion = baseJointPostion;

                    filteredJointPostion = filteredJointPostion * (1.0f - DataSmothingFactor) + (previousFilterdJointPostion + previousTrens) * DataSmothingFactor;
                    differanceVector = filteredJointPostion - previousFilterdJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

            }
            Vector2 potentialNewPOstion = new Vector2(filteredJointPostion.X + currentTrend.X * PredicationFacrtor, filteredJointPostion.Y + currentTrend.Y * PredicationFacrtor);
            savedBasePostion = baseJointPostion;
            savedFilterdJointPostion = filteredJointPostion; ;
            savedTrend = currentTrend;
            framcount++;
            return potentialNewPOstion;

        }
     
            
      
      

     

        public void Dispose()
        {
            KinectSensor.KinectSensors.Dispose();         
            
        }
        #region DepthFrame 
        private void ConvertDepthFram(short[] depthFrame16)
        {
            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
            {
                int user = depthFrame16[i16] & 0x07;
                int player = depthFrame16[i16] & DepthImageFrame.PlayerIndexBitmask;
                int realDepth = (depthFrame16[i16] >> 3);
                byte intensity = (byte)(255 - (255 * realDepth / 0xaaaa));
                depthFrame32[i32] = (byte)(intensity);
                depthFrame32[i32 + 1] = (byte)(intensity);
                depthFrame32[i32 + 2] = (byte)(intensity);
                depthFrame32[i32 + 3] = (byte)(intensity);

            }
        }
        #endregion





        public bool Tracking { get; set; }
    }
}
